/*************************************************************************\

  Copyright 1999 The University of North Carolina at Chapel Hill.
  All Rights Reserved.

  Permission to use, copy, modify and distribute this software and its
  documentation for educational, research and non-profit purposes, without
  fee, and without a written agreement is hereby granted, provided that the
  above copyright notice and the following three paragraphs appear in all
  copies.

  IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
  LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
  CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
  USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
  OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
  DAMAGES.

  THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
  PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
  NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.

  The authors may be contacted via:

  US Mail:             S. Gottschalk
                       Department of Computer Science
                       Sitterson Hall, CB #3175
                       University of N. Carolina
                       Chapel Hill, NC 27599-3175

  Phone:               (919)962-1749

  EMail:               geom@cs.unc.edu


\**************************************************************************/

#pragma once

#include <math.h>
#include <stdio.h>
#include "PQP_Compile.h"
namespace PQP
{

#ifndef M_PI
    const PQP_REAL M_PI = (PQP_REAL)3.14159265359;
#endif

#ifdef gnu
#include "zzzz.h"

#ifdef hppa
#define myfabs(x) \
    ({double __value, __arg = (x); \
        asm("fabs,dbl %1, %0": "=f" (__value): "f" (__arg)); \
        __value; \
    });
#endif

#ifdef mips
#define myfabs(x) \
    ({double __value, __arg = (x); \
        asm("abs.d %0, %1": "=f" (__value): "f" (__arg)); \
        __value; \
    });
#endif

#else

#define myfabs(x) ((x < 0) ? -x : x)

#endif

    class MatVec
    {
    public:

        inline
        void
        Mprintg(const PQP_REAL M[3][3])
        {
            printf("%g %g %g\n%g %g %g\n%g %g %g\n",
                   M[0][0], M[0][1], M[0][2],
                   M[1][0], M[1][1], M[1][2],
                   M[2][0], M[2][1], M[2][2]);
        }


        inline
        void
        Mfprint(FILE* f, const PQP_REAL M[3][3])
        {
            fprintf(f, "%g %g %g\n%g %g %g\n%g %g %g\n",
                    M[0][0], M[0][1], M[0][2],
                    M[1][0], M[1][1], M[1][2],
                    M[2][0], M[2][1], M[2][2]);
        }

        inline
        void
        Mprint(const PQP_REAL M[3][3])
        {
            printf("%g %g %g\n%g %g %g\n%g %g %g\n",
                   M[0][0], M[0][1], M[0][2],
                   M[1][0], M[1][1], M[1][2],
                   M[2][0], M[2][1], M[2][2]);
        }

        inline
        void
        Vprintg(const PQP_REAL V[3])
        {
            printf("%g %g %g\n", V[0], V[1], V[2]);
        }

        inline
        void
        Vfprint(FILE* f, const PQP_REAL V[3])
        {
            fprintf(f, "%g %g %g\n", V[0], V[1], V[2]);
        }

        inline
        void
        Vprint(const PQP_REAL V[3])
        {
            printf("%g %g %g\n", V[0], V[1], V[2]);
        }

        inline
        void
        Midentity(PQP_REAL M[3][3])
        {
            M[0][0] = M[1][1] = M[2][2] = 1.0;
            M[0][1] = M[1][2] = M[2][0] = 0.0;
            M[0][2] = M[1][0] = M[2][1] = 0.0;
        }

        inline
        void
        Videntity(PQP_REAL T[3])
        {
            T[0] = T[1] = T[2] = 0.0;
        }

        inline
        void
        McM(PQP_REAL Mr[3][3], const PQP_REAL M[3][3])
        {
            Mr[0][0] = M[0][0];
            Mr[0][1] = M[0][1];
            Mr[0][2] = M[0][2];
            Mr[1][0] = M[1][0];
            Mr[1][1] = M[1][1];
            Mr[1][2] = M[1][2];
            Mr[2][0] = M[2][0];
            Mr[2][1] = M[2][1];
            Mr[2][2] = M[2][2];
        }

        inline
        void
        MTcM(PQP_REAL Mr[3][3], const PQP_REAL M[3][3])
        {
            Mr[0][0] = M[0][0];
            Mr[1][0] = M[0][1];
            Mr[2][0] = M[0][2];
            Mr[0][1] = M[1][0];
            Mr[1][1] = M[1][1];
            Mr[2][1] = M[1][2];
            Mr[0][2] = M[2][0];
            Mr[1][2] = M[2][1];
            Mr[2][2] = M[2][2];
        }

        inline
        void
        VcV(PQP_REAL Vr[3], const PQP_REAL V[3])
        {
            Vr[0] = V[0];
            Vr[1] = V[1];
            Vr[2] = V[2];
        }

        inline
        void
        McolcV(PQP_REAL Vr[3], const PQP_REAL M[3][3], int c)
        {
            Vr[0] = M[0][c];
            Vr[1] = M[1][c];
            Vr[2] = M[2][c];
        }

        inline
        void
        McolcMcol(PQP_REAL Mr[3][3], int cr, const PQP_REAL M[3][3], int c)
        {
            Mr[0][cr] = M[0][c];
            Mr[1][cr] = M[1][c];
            Mr[2][cr] = M[2][c];
        }

        inline
        void
        MxMpV(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3], const PQP_REAL T[3])
        {
            Mr[0][0] = (M1[0][0] * M2[0][0] +
                        M1[0][1] * M2[1][0] +
                        M1[0][2] * M2[2][0] +
                        T[0]);
            Mr[1][0] = (M1[1][0] * M2[0][0] +
                        M1[1][1] * M2[1][0] +
                        M1[1][2] * M2[2][0] +
                        T[1]);
            Mr[2][0] = (M1[2][0] * M2[0][0] +
                        M1[2][1] * M2[1][0] +
                        M1[2][2] * M2[2][0] +
                        T[2]);
            Mr[0][1] = (M1[0][0] * M2[0][1] +
                        M1[0][1] * M2[1][1] +
                        M1[0][2] * M2[2][1] +
                        T[0]);
            Mr[1][1] = (M1[1][0] * M2[0][1] +
                        M1[1][1] * M2[1][1] +
                        M1[1][2] * M2[2][1] +
                        T[1]);
            Mr[2][1] = (M1[2][0] * M2[0][1] +
                        M1[2][1] * M2[1][1] +
                        M1[2][2] * M2[2][1] +
                        T[2]);
            Mr[0][2] = (M1[0][0] * M2[0][2] +
                        M1[0][1] * M2[1][2] +
                        M1[0][2] * M2[2][2] +
                        T[0]);
            Mr[1][2] = (M1[1][0] * M2[0][2] +
                        M1[1][1] * M2[1][2] +
                        M1[1][2] * M2[2][2] +
                        T[1]);
            Mr[2][2] = (M1[2][0] * M2[0][2] +
                        M1[2][1] * M2[1][2] +
                        M1[2][2] * M2[2][2] +
                        T[2]);
        }

        inline
        void
        MxM(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
        {
            Mr[0][0] = (M1[0][0] * M2[0][0] +
                        M1[0][1] * M2[1][0] +
                        M1[0][2] * M2[2][0]);
            Mr[1][0] = (M1[1][0] * M2[0][0] +
                        M1[1][1] * M2[1][0] +
                        M1[1][2] * M2[2][0]);
            Mr[2][0] = (M1[2][0] * M2[0][0] +
                        M1[2][1] * M2[1][0] +
                        M1[2][2] * M2[2][0]);
            Mr[0][1] = (M1[0][0] * M2[0][1] +
                        M1[0][1] * M2[1][1] +
                        M1[0][2] * M2[2][1]);
            Mr[1][1] = (M1[1][0] * M2[0][1] +
                        M1[1][1] * M2[1][1] +
                        M1[1][2] * M2[2][1]);
            Mr[2][1] = (M1[2][0] * M2[0][1] +
                        M1[2][1] * M2[1][1] +
                        M1[2][2] * M2[2][1]);
            Mr[0][2] = (M1[0][0] * M2[0][2] +
                        M1[0][1] * M2[1][2] +
                        M1[0][2] * M2[2][2]);
            Mr[1][2] = (M1[1][0] * M2[0][2] +
                        M1[1][1] * M2[1][2] +
                        M1[1][2] * M2[2][2]);
            Mr[2][2] = (M1[2][0] * M2[0][2] +
                        M1[2][1] * M2[1][2] +
                        M1[2][2] * M2[2][2]);
        }


        inline
        void
        MxMT(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
        {
            Mr[0][0] = (M1[0][0] * M2[0][0] +
                        M1[0][1] * M2[0][1] +
                        M1[0][2] * M2[0][2]);
            Mr[1][0] = (M1[1][0] * M2[0][0] +
                        M1[1][1] * M2[0][1] +
                        M1[1][2] * M2[0][2]);
            Mr[2][0] = (M1[2][0] * M2[0][0] +
                        M1[2][1] * M2[0][1] +
                        M1[2][2] * M2[0][2]);
            Mr[0][1] = (M1[0][0] * M2[1][0] +
                        M1[0][1] * M2[1][1] +
                        M1[0][2] * M2[1][2]);
            Mr[1][1] = (M1[1][0] * M2[1][0] +
                        M1[1][1] * M2[1][1] +
                        M1[1][2] * M2[1][2]);
            Mr[2][1] = (M1[2][0] * M2[1][0] +
                        M1[2][1] * M2[1][1] +
                        M1[2][2] * M2[1][2]);
            Mr[0][2] = (M1[0][0] * M2[2][0] +
                        M1[0][1] * M2[2][1] +
                        M1[0][2] * M2[2][2]);
            Mr[1][2] = (M1[1][0] * M2[2][0] +
                        M1[1][1] * M2[2][1] +
                        M1[1][2] * M2[2][2]);
            Mr[2][2] = (M1[2][0] * M2[2][0] +
                        M1[2][1] * M2[2][1] +
                        M1[2][2] * M2[2][2]);
        }

        inline
        void
        MTxM(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
        {
            Mr[0][0] = (M1[0][0] * M2[0][0] +
                        M1[1][0] * M2[1][0] +
                        M1[2][0] * M2[2][0]);
            Mr[1][0] = (M1[0][1] * M2[0][0] +
                        M1[1][1] * M2[1][0] +
                        M1[2][1] * M2[2][0]);
            Mr[2][0] = (M1[0][2] * M2[0][0] +
                        M1[1][2] * M2[1][0] +
                        M1[2][2] * M2[2][0]);
            Mr[0][1] = (M1[0][0] * M2[0][1] +
                        M1[1][0] * M2[1][1] +
                        M1[2][0] * M2[2][1]);
            Mr[1][1] = (M1[0][1] * M2[0][1] +
                        M1[1][1] * M2[1][1] +
                        M1[2][1] * M2[2][1]);
            Mr[2][1] = (M1[0][2] * M2[0][1] +
                        M1[1][2] * M2[1][1] +
                        M1[2][2] * M2[2][1]);
            Mr[0][2] = (M1[0][0] * M2[0][2] +
                        M1[1][0] * M2[1][2] +
                        M1[2][0] * M2[2][2]);
            Mr[1][2] = (M1[0][1] * M2[0][2] +
                        M1[1][1] * M2[1][2] +
                        M1[2][1] * M2[2][2]);
            Mr[2][2] = (M1[0][2] * M2[0][2] +
                        M1[1][2] * M2[1][2] +
                        M1[2][2] * M2[2][2]);
        }

        inline
        void
        MxV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3])
        {
            Vr[0] = (M1[0][0] * V1[0] +
                     M1[0][1] * V1[1] +
                     M1[0][2] * V1[2]);
            Vr[1] = (M1[1][0] * V1[0] +
                     M1[1][1] * V1[1] +
                     M1[1][2] * V1[2]);
            Vr[2] = (M1[2][0] * V1[0] +
                     M1[2][1] * V1[1] +
                     M1[2][2] * V1[2]);
        }


        inline
        void
        MxVpV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3], const PQP_REAL V2[3])
        {
            Vr[0] = (M1[0][0] * V1[0] +
                     M1[0][1] * V1[1] +
                     M1[0][2] * V1[2] +
                     V2[0]);
            Vr[1] = (M1[1][0] * V1[0] +
                     M1[1][1] * V1[1] +
                     M1[1][2] * V1[2] +
                     V2[1]);
            Vr[2] = (M1[2][0] * V1[0] +
                     M1[2][1] * V1[1] +
                     M1[2][2] * V1[2] +
                     V2[2]);
        }


        inline
        void
        sMxVpV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3], const PQP_REAL V2[3])
        {
            Vr[0] = s1 * (M1[0][0] * V1[0] +
                          M1[0][1] * V1[1] +
                          M1[0][2] * V1[2]) +
                    V2[0];
            Vr[1] = s1 * (M1[1][0] * V1[0] +
                          M1[1][1] * V1[1] +
                          M1[1][2] * V1[2]) +
                    V2[1];
            Vr[2] = s1 * (M1[2][0] * V1[0] +
                          M1[2][1] * V1[1] +
                          M1[2][2] * V1[2]) +
                    V2[2];
        }

        inline
        void
        MTxV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3])
        {
            Vr[0] = (M1[0][0] * V1[0] +
                     M1[1][0] * V1[1] +
                     M1[2][0] * V1[2]);
            Vr[1] = (M1[0][1] * V1[0] +
                     M1[1][1] * V1[1] +
                     M1[2][1] * V1[2]);
            Vr[2] = (M1[0][2] * V1[0] +
                     M1[1][2] * V1[1] +
                     M1[2][2] * V1[2]);
        }

        inline
        void
        sMTxV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3])
        {
            Vr[0] = s1 * (M1[0][0] * V1[0] +
                          M1[1][0] * V1[1] +
                          M1[2][0] * V1[2]);
            Vr[1] = s1 * (M1[0][1] * V1[0] +
                          M1[1][1] * V1[1] +
                          M1[2][1] * V1[2]);
            Vr[2] = s1 * (M1[0][2] * V1[0] +
                          M1[1][2] * V1[1] +
                          M1[2][2] * V1[2]);
        }

        inline
        void
        sMxV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3])
        {
            Vr[0] = s1 * (M1[0][0] * V1[0] +
                          M1[0][1] * V1[1] +
                          M1[0][2] * V1[2]);
            Vr[1] = s1 * (M1[1][0] * V1[0] +
                          M1[1][1] * V1[1] +
                          M1[1][2] * V1[2]);
            Vr[2] = s1 * (M1[2][0] * V1[0] +
                          M1[2][1] * V1[1] +
                          M1[2][2] * V1[2]);
        }


        inline
        void
        VmV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
        {
            Vr[0] = V1[0] - V2[0];
            Vr[1] = V1[1] - V2[1];
            Vr[2] = V1[2] - V2[2];
        }

        inline
        void
        VpV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
        {
            Vr[0] = V1[0] + V2[0];
            Vr[1] = V1[1] + V2[1];
            Vr[2] = V1[2] + V2[2];
        }

        inline
        void
        VpVxS(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3], PQP_REAL s)
        {
            Vr[0] = V1[0] + V2[0] * s;
            Vr[1] = V1[1] + V2[1] * s;
            Vr[2] = V1[2] + V2[2] * s;
        }

        inline
        void
        MskewV(PQP_REAL M[3][3], const PQP_REAL v[3])
        {
            M[0][0] = M[1][1] = M[2][2] = 0.0;
            M[1][0] = v[2];
            M[0][1] = -v[2];
            M[0][2] = v[1];
            M[2][0] = -v[1];
            M[1][2] = -v[0];
            M[2][1] = v[0];
        }


        inline
        void
        VcrossV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
        {
            Vr[0] = V1[1] * V2[2] - V1[2] * V2[1];
            Vr[1] = V1[2] * V2[0] - V1[0] * V2[2];
            Vr[2] = V1[0] * V2[1] - V1[1] * V2[0];
        }

        inline
        PQP_REAL
        Vlength(PQP_REAL V[3])
        {
            return sqrt(V[0] * V[0] + V[1] * V[1] + V[2] * V[2]);
        }

        inline
        void
        Vnormalize(PQP_REAL V[3])
        {
            PQP_REAL d = (PQP_REAL)1.0 / sqrt(V[0] * V[0] + V[1] * V[1] + V[2] * V[2]);
            V[0] *= d;
            V[1] *= d;
            V[2] *= d;
        }

        inline
        PQP_REAL
        VdotV(const PQP_REAL V1[3], const PQP_REAL V2[3])
        {
            return (V1[0] * V2[0] + V1[1] * V2[1] + V1[2] * V2[2]);
        }

        inline
        PQP_REAL
        VdistV2(const PQP_REAL V1[3], const PQP_REAL V2[3])
        {
            return ((V1[0] - V2[0]) * (V1[0] - V2[0]) +
                    (V1[1] - V2[1]) * (V1[1] - V2[1]) +
                    (V1[2] - V2[2]) * (V1[2] - V2[2]));
        }

        inline
        void
        VxS(PQP_REAL Vr[3], const PQP_REAL V[3], PQP_REAL s)
        {
            Vr[0] = V[0] * s;
            Vr[1] = V[1] * s;
            Vr[2] = V[2] * s;
        }

        inline
        void
        MRotZ(PQP_REAL Mr[3][3], PQP_REAL t)
        {
            Mr[0][0] = cos(t);
            Mr[1][0] = sin(t);
            Mr[0][1] = -Mr[1][0];
            Mr[1][1] = Mr[0][0];
            Mr[2][0] = Mr[2][1] = 0.0;
            Mr[0][2] = Mr[1][2] = 0.0;
            Mr[2][2] = 1.0;
        }

        inline
        void
        MRotX(PQP_REAL Mr[3][3], PQP_REAL t)
        {
            Mr[1][1] = cos(t);
            Mr[2][1] = sin(t);
            Mr[1][2] = -Mr[2][1];
            Mr[2][2] = Mr[1][1];
            Mr[0][1] = Mr[0][2] = 0.0;
            Mr[1][0] = Mr[2][0] = 0.0;
            Mr[0][0] = 1.0;
        }

        inline
        void
        MRotY(PQP_REAL Mr[3][3], PQP_REAL t)
        {
            Mr[2][2] = cos(t);
            Mr[0][2] = sin(t);
            Mr[2][0] = -Mr[0][2];
            Mr[0][0] = Mr[2][2];
            Mr[1][2] = Mr[1][0] = 0.0;
            Mr[2][1] = Mr[0][1] = 0.0;
            Mr[1][1] = 1.0;
        }

        inline
        void
        MVtoOGL(double oglm[16], const PQP_REAL R[3][3], const PQP_REAL T[3])
        {
            oglm[0] = (double)R[0][0];
            oglm[1] = (double)R[1][0];
            oglm[2] = (double)R[2][0];
            oglm[3] = 0.0;
            oglm[4] = (double)R[0][1];
            oglm[5] = (double)R[1][1];
            oglm[6] = (double)R[2][1];
            oglm[7] = 0.0;
            oglm[8] = (double)R[0][2];
            oglm[9] = (double)R[1][2];
            oglm[10] = (double)R[2][2];
            oglm[11] = 0.0;
            oglm[12] = (double)T[0];
            oglm[13] = (double)T[1];
            oglm[14] = (double)T[2];
            oglm[15] = 1.0;
        }

        inline
        void
        OGLtoMV(PQP_REAL R[3][3], PQP_REAL T[3], const double oglm[16])
        {
            R[0][0] = (PQP_REAL)oglm[0];
            R[1][0] = (PQP_REAL)oglm[1];
            R[2][0] = (PQP_REAL)oglm[2];

            R[0][1] = (PQP_REAL)oglm[4];
            R[1][1] = (PQP_REAL)oglm[5];
            R[2][1] = (PQP_REAL)oglm[6];

            R[0][2] = (PQP_REAL)oglm[8];
            R[1][2] = (PQP_REAL)oglm[9];
            R[2][2] = (PQP_REAL)oglm[10];

            T[0] = (PQP_REAL)oglm[12];
            T[1] = (PQP_REAL)oglm[13];
            T[2] = (PQP_REAL)oglm[14];
        }

        // taken from quatlib, written by Richard Holloway
        static const int QX = 0;
        static const int QY = 1;
        static const int QZ = 2;
        static const int QW = 3;

        inline
        void
        MRotQ(PQP_REAL destMatrix[3][3], PQP_REAL srcQuat[4])
        {
            PQP_REAL  s;
            PQP_REAL  xs, ys, zs,
                      wx, wy, wz,
                      xx, xy, xz,
                      yy, yz, zz;

            /*
             * For unit srcQuat, just set s = 2.0; or set xs = srcQuat[QX] +
             *   srcQuat[QX], etc.
             */

            s = (PQP_REAL)2.0 / (srcQuat[QX] * srcQuat[QX] + srcQuat[QY] * srcQuat[QY] +
                                 srcQuat[QZ] * srcQuat[QZ] + srcQuat[QW] * srcQuat[QW]);

            xs = srcQuat[QX] * s;
            ys = srcQuat[QY] * s;
            zs = srcQuat[QZ] * s;
            wx = srcQuat[QW] * xs;
            wy = srcQuat[QW] * ys;
            wz = srcQuat[QW] * zs;
            xx = srcQuat[QX] * xs;
            xy = srcQuat[QX] * ys;
            xz = srcQuat[QX] * zs;
            yy = srcQuat[QY] * ys;
            yz = srcQuat[QY] * zs;
            zz = srcQuat[QZ] * zs;

            destMatrix[QX][QX] = (PQP_REAL)1.0 - (yy + zz);
            destMatrix[QX][QY] = xy + wz;
            destMatrix[QX][QZ] = xz - wy;

            destMatrix[QY][QX] = xy - wz;
            destMatrix[QY][QY] = (PQP_REAL)1.0 - (xx + zz);
            destMatrix[QY][QZ] = yz + wx;

            destMatrix[QZ][QX] = xz + wy;
            destMatrix[QZ][QY] = yz - wx;
            destMatrix[QZ][QZ] = (PQP_REAL)1.0 - (xx + yy);
        }

        inline
        void
        Mqinverse(PQP_REAL Mr[3][3], PQP_REAL m[3][3])
        {
            int i, j;

            for (i = 0; i < 3; i++)
                for (j = 0; j < 3; j++)
                {
                    int i1 = (i + 1) % 3;
                    int i2 = (i + 2) % 3;
                    int j1 = (j + 1) % 3;
                    int j2 = (j + 2) % 3;
                    Mr[i][j] = (m[j1][i1] * m[j2][i2] - m[j1][i2] * m[j2][i1]);
                }
        }

        // Meigen from Numerical Recipes in C

#if 0

#define rfabs(x) ((x < 0) ? -x : x)

#define ROT(a,i,j,k,l) g=a[i][j]; h=a[k][l]; a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau);

        int
        inline
        Meigen(PQP_REAL vout[3][3], PQP_REAL dout[3], PQP_REAL a[3][3])
        {
            int i;
            PQP_REAL tresh, theta, tau, t, sm, s, h, g, c;
            int nrot;
            PQP_REAL b[3];
            PQP_REAL z[3];
            PQP_REAL v[3][3];
            PQP_REAL d[3];

            v[0][0] = v[1][1] = v[2][2] = 1.0;
            v[0][1] = v[1][2] = v[2][0] = 0.0;
            v[0][2] = v[1][0] = v[2][1] = 0.0;

            b[0] = a[0][0];
            d[0] = a[0][0];
            z[0] = 0.0;
            b[1] = a[1][1];
            d[1] = a[1][1];
            z[1] = 0.0;
            b[2] = a[2][2];
            d[2] = a[2][2];
            z[2] = 0.0;

            nrot = 0;


            for (i = 0; i < 50; i++)
            {

                printf("2\n");

                sm = 0.0;
                sm += fabs(a[0][1]);
                sm += fabs(a[0][2]);
                sm += fabs(a[1][2]);

                if (sm == 0.0)
                {
                    McM(vout, v);
                    VcV(dout, d);
                    return i;
                }

                if (i < 3)
                {
                    tresh = 0.2 * sm / (3 * 3);
                }
                else
                {
                    tresh = 0.0;
                }

                {
                    g = 100.0 * rfabs(a[0][1]);

                    if (i > 3 && rfabs(d[0]) + g == rfabs(d[0]) && rfabs(d[1]) + g == rfabs(d[1]))
                    {
                        a[0][1] = 0.0;
                    }
                    else if (rfabs(a[0][1]) > tresh)
                    {
                        h = d[1] - d[0];

                        if (rfabs(h) + g == rfabs(h))
                        {
                            t = (a[0][1]) / h;
                        }
                        else
                        {
                            theta = 0.5 * h / (a[0][1]);
                            t = 1.0 / (rfabs(theta) + sqrt(1.0 + theta * theta));

                            if (theta < 0.0)
                            {
                                t = -t;
                            }
                        }

                        c = 1.0 / sqrt(1 + t * t);
                        s = t * c;
                        tau = s / (1.0 + c);
                        h = t * a[0][1];
                        z[0] -= h;
                        z[1] += h;
                        d[0] -= h;
                        d[1] += h;
                        a[0][1] = 0.0;
                        ROT(a, 0, 2, 1, 2);
                        ROT(v, 0, 0, 0, 1);
                        ROT(v, 1, 0, 1, 1);
                        ROT(v, 2, 0, 2, 1);
                        nrot++;
                    }
                }

                {
                    g = 100.0 * rfabs(a[0][2]);

                    if (i > 3 && rfabs(d[0]) + g == rfabs(d[0]) && rfabs(d[2]) + g == rfabs(d[2]))
                    {
                        a[0][2] = 0.0;
                    }
                    else if (rfabs(a[0][2]) > tresh)
                    {
                        h = d[2] - d[0];

                        if (rfabs(h) + g == rfabs(h))
                        {
                            t = (a[0][2]) / h;
                        }
                        else
                        {
                            theta = 0.5 * h / (a[0][2]);
                            t = 1.0 / (rfabs(theta) + sqrt(1.0 + theta * theta));

                            if (theta < 0.0)
                            {
                                t = -t;
                            }
                        }

                        c = 1.0 / sqrt(1 + t * t);
                        s = t * c;
                        tau = s / (1.0 + c);
                        h = t * a[0][2];
                        z[0] -= h;
                        z[2] += h;
                        d[0] -= h;
                        d[2] += h;
                        a[0][2] = 0.0;
                        ROT(a, 0, 1, 1, 2);
                        ROT(v, 0, 0, 0, 2);
                        ROT(v, 1, 0, 1, 2);
                        ROT(v, 2, 0, 2, 2);
                        nrot++;
                    }
                }


                {
                    g = 100.0 * rfabs(a[1][2]);

                    if (i > 3 && rfabs(d[1]) + g == rfabs(d[1]) && rfabs(d[2]) + g == rfabs(d[2]))
                    {
                        a[1][2] = 0.0;
                    }
                    else if (rfabs(a[1][2]) > tresh)
                    {
                        h = d[2] - d[1];

                        if (rfabs(h) + g == rfabs(h))
                        {
                            t = (a[1][2]) / h;
                        }
                        else
                        {
                            theta = 0.5 * h / (a[1][2]);
                            t = 1.0 / (rfabs(theta) + sqrt(1.0 + theta * theta));

                            if (theta < 0.0)
                            {
                                t = -t;
                            }
                        }

                        c = 1.0 / sqrt(1 + t * t);
                        s = t * c;
                        tau = s / (1.0 + c);
                        h = t * a[1][2];
                        z[1] -= h;
                        z[2] += h;
                        d[1] -= h;
                        d[2] += h;
                        a[1][2] = 0.0;
                        ROT(a, 0, 1, 0, 2);
                        ROT(v, 0, 1, 0, 2);
                        ROT(v, 1, 1, 1, 2);
                        ROT(v, 2, 1, 2, 2);
                        nrot++;
                    }
                }

                b[0] += z[0];
                d[0] = b[0];
                z[0] = 0.0;
                b[1] += z[1];
                d[1] = b[1];
                z[1] = 0.0;
                b[2] += z[2];
                d[2] = b[2];
                z[2] = 0.0;

            }

            fprintf(stderr, "eigen: too many iterations in Jacobi transform (%d).\n", i);

            return i;
        }

#else



#define ROTATE(a,i,j,k,l) g=a[i][j]; h=a[k][l]; a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau);

        void
        inline
        Meigen(PQP_REAL vout[3][3], PQP_REAL dout[3], PQP_REAL a[3][3])
        {
            int n = 3;
            int j, iq, ip, i;
            PQP_REAL tresh, theta, tau, t, sm, s, h, g, c;
            int nrot;
            PQP_REAL b[3];
            PQP_REAL z[3];
            PQP_REAL v[3][3];
            PQP_REAL d[3];

            Midentity(v);

            for (ip = 0; ip < n; ip++)
            {
                b[ip] = a[ip][ip];
                d[ip] = a[ip][ip];
                z[ip] = 0.0;
            }

            nrot = 0;

            for (i = 0; i < 50; i++)
            {

                sm = 0.0;

                for (ip = 0; ip < n; ip++) for (iq = ip + 1; iq < n; iq++)
                    {
                        sm += fabs(a[ip][iq]);
                    }

                if (sm == 0.0)
                {
                    McM(vout, v);
                    VcV(dout, d);
                    return;
                }


                if (i < 3)
                {
                    tresh = (PQP_REAL)0.2 * sm / (n * n);
                }
                else
                {
                    tresh = 0.0;
                }

                for (ip = 0; ip < n; ip++) for (iq = ip + 1; iq < n; iq++)
                    {
                        g = (PQP_REAL)100.0 * fabs(a[ip][iq]);

                        if (i > 3 &&
                            fabs(d[ip]) + g == fabs(d[ip]) &&
                            fabs(d[iq]) + g == fabs(d[iq]))
                        {
                            a[ip][iq] = 0.0;
                        }
                        else if (fabs(a[ip][iq]) > tresh)
                        {
                            h = d[iq] - d[ip];

                            if (fabs(h) + g == fabs(h))
                            {
                                t = (a[ip][iq]) / h;
                            }
                            else
                            {
                                theta = (PQP_REAL)0.5 * h / (a[ip][iq]);
                                t = (PQP_REAL)(1.0 / (fabs(theta) + sqrt(1.0 + theta * theta)));

                                if (theta < 0.0)
                                {
                                    t = -t;
                                }
                            }

                            c = (PQP_REAL)1.0 / sqrt(1 + t * t);
                            s = t * c;
                            tau = s / ((PQP_REAL)1.0 + c);
                            h = t * a[ip][iq];
                            z[ip] -= h;
                            z[iq] += h;
                            d[ip] -= h;
                            d[iq] += h;
                            a[ip][iq] = 0.0;

                            for (j = 0; j < ip; j++)
                            {
                                ROTATE(a, j, ip, j, iq);
                            }

                            for (j = ip + 1; j < iq; j++)
                            {
                                ROTATE(a, ip, j, j, iq);
                            }

                            for (j = iq + 1; j < n; j++)
                            {
                                ROTATE(a, ip, j, iq, j);
                            }

                            for (j = 0; j < n; j++)
                            {
                                ROTATE(v, j, ip, j, iq);
                            }

                            nrot++;
                        }
                    }

                for (ip = 0; ip < n; ip++)
                {
                    b[ip] += z[ip];
                    d[ip] = b[ip];
                    z[ip] = 0.0;
                }
            }

            fprintf(stderr, "eigen: too many iterations in Jacobi transform.\n");

            return;
        }


#endif

    };

} // namespace

// MATVEC_H
